﻿namespace JelloPhysics
{
    using Microsoft.Xna.Framework;
    using System;
    using System.Collections.Generic;

    public class SpringBody : Body
    {
        protected float mEdgeSpringDamp;
        protected float mEdgeSpringK;
        protected bool mShapeMatchingOn;
        protected float mShapeSpringDamp;
        protected float mShapeSpringK;
        protected List<InternalSpring> mSprings;

        public SpringBody(World w, ClosedShape shape, float massPerPoint, float edgeSpringK, float edgeSpringDamp, Vector2 pos, float angleinRadians, Vector2 scale, bool kinematic) : base(w, shape, massPerPoint, pos, angleinRadians, scale, kinematic)
        {
            this.mShapeMatchingOn = true;
            this.mShapeMatchingOn = false;
            this.mSprings = new List<InternalSpring>();
            base.setPositionAngle(pos, angleinRadians, scale);
            this.mEdgeSpringK = edgeSpringK;
            this.mEdgeSpringDamp = edgeSpringDamp;
            this.mShapeSpringK = 0f;
            this.mShapeSpringDamp = 0f;
            this._buildDefaultSprings();
        }

        public SpringBody(World w, ClosedShape shape, float massPerPoint, float shapeSpringK, float shapeSpringDamp, float edgeSpringK, float edgeSpringDamp, Vector2 pos, float angleinRadians, Vector2 scale, bool kinematic) : base(w, shape, massPerPoint, pos, angleinRadians, scale, kinematic)
        {
            this.mShapeMatchingOn = true;
            this.mSprings = new List<InternalSpring>();
            base.setPositionAngle(pos, angleinRadians, scale);
            this.mShapeMatchingOn = true;
            this.mShapeSpringK = shapeSpringK;
            this.mShapeSpringDamp = shapeSpringDamp;
            this.mEdgeSpringK = edgeSpringK;
            this.mEdgeSpringDamp = edgeSpringDamp;
            this._buildDefaultSprings();
        }

        private void _buildDefaultSprings()
        {
            for (int i = 0; i < base.mPointMasses.Count; i++)
            {
                if (i < (base.mPointMasses.Count - 1))
                {
                    this.addInternalSpring(i, i + 1, this.mEdgeSpringK, this.mEdgeSpringDamp);
                }
                else
                {
                    this.addInternalSpring(i, 0, this.mEdgeSpringK, this.mEdgeSpringDamp);
                }
            }
        }

        public override void accumulateInternalForces()
        {
            base.accumulateInternalForces();
            Vector2 forceOut = new Vector2();
            for (int i = 0; i < this.mSprings.Count; i++)
            {
                InternalSpring spring = this.mSprings[i];
                VectorTools.calculateSpringForce(ref base.mPointMasses[spring.pointMassA].Position, ref base.mPointMasses[spring.pointMassA].Velocity, ref base.mPointMasses[spring.pointMassB].Position, ref base.mPointMasses[spring.pointMassB].Velocity, spring.springD, spring.springK, spring.damping, ref forceOut);
                base.mPointMasses[spring.pointMassA].Force.X += forceOut.X;
                base.mPointMasses[spring.pointMassA].Force.Y += forceOut.Y;
                base.mPointMasses[spring.pointMassB].Force.X -= forceOut.X;
                base.mPointMasses[spring.pointMassB].Force.Y -= forceOut.Y;
            }
            if (this.mShapeMatchingOn)
            {
                base.mBaseShape.transformVertices(ref this.mDerivedPos, base.mDerivedAngle, ref this.mScale, ref this.mGlobalShape);
                for (int j = 0; j < base.mPointMasses.Count; j++)
                {
                    if (this.mShapeSpringK > 0f)
                    {
                        if (!base.mKinematic)
                        {
                            VectorTools.calculateSpringForce(ref base.mPointMasses[j].Position, ref base.mPointMasses[j].Velocity, ref base.mGlobalShape[j], ref base.mPointMasses[j].Velocity, 0f, this.mShapeSpringK, this.mShapeSpringDamp, ref forceOut);
                        }
                        else
                        {
                            Vector2 zero = Vector2.Zero;
                            VectorTools.calculateSpringForce(ref base.mPointMasses[j].Position, ref base.mPointMasses[j].Velocity, ref base.mGlobalShape[j], ref zero, 0f, this.mShapeSpringK, this.mShapeSpringDamp, ref forceOut);
                        }
                        base.mPointMasses[j].Force.X += forceOut.X;
                        base.mPointMasses[j].Force.Y += forceOut.Y;
                    }
                }
            }
        }

        public void addInternalSpring(int pointA, int pointB, float springK, float damping)
        {
            float d = (base.mPointMasses[pointB].Position - base.mPointMasses[pointA].Position).Length();
            InternalSpring item = new InternalSpring(pointA, pointB, d, springK, damping);
            this.mSprings.Add(item);
        }

        public void clearAllSprings()
        {
            this.mSprings.Clear();
            this._buildDefaultSprings();
        }

        public float getSpringDamping(int springID)
        {
            int num = base.mPointMasses.Count + springID;
            return this.mSprings[num].damping;
        }

        public float getSpringK(int springID)
        {
            int num = base.mPointMasses.Count + springID;
            return this.mSprings[num].springK;
        }

        public void setEdgeSpringConstants(float edgeSpringK, float edgeSpringDamp)
        {
            for (int i = 0; i < base.mPointMasses.Count; i++)
            {
                this.mSprings[i].springK = edgeSpringK;
                this.mSprings[i].damping = edgeSpringDamp;
            }
        }

        public void setShapeMatching(bool onoff)
        {
            this.mShapeMatchingOn = onoff;
        }

        public void setShapeMatchingConstants(float springK, float damping)
        {
            this.mShapeSpringK = springK;
            this.mShapeSpringDamp = damping;
        }

        public void setSpringConstants(int springID, float springK, float springDamp)
        {
            int num = base.mPointMasses.Count + springID;
            this.mSprings[num].springK = springK;
            this.mSprings[num].damping = springDamp;
        }
    }
}

